/***************************************************************************
 创建者: hualei
 开始时间: 2018.10.17
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2018.10.17 类的具体实现代码编写

 ***************************************************************************/
#include "setodominstructionlogic.h"
#include "programset.h"
#include "mccontrollerthread.h"
#include "programengine.h"
//#include "../HmiGsoapQt/gSoapFoundation/hmiAPI.h"
#include "GeneralDefine.h"
#include "messagelog.h"
#define D_SET_ODOMETER_COVE_RATIO 0.1

SetOdomInstructionLogic::SetOdomInstructionLogic(int robotIdIn, ProgramSet *programSetIn, ProgramEngine *programEngineIn, McControllerThread *programMcControllerIn)
{
    robotId = robotIdIn;
    programSet = programSetIn;
    programEngine = programEngineIn;
    programMcController = programMcControllerIn;

}

E_PROGRAM_LOOP_RESULT SetOdomInstructionLogic::startSetOdom(int programNumIn, const OdometryResetInstruction &instructionIn, int lineIndex)
{
    if(instructionIn.instructionType != SET_ODOM)
    {
       return E_PROGRAM_LOOP_RESULT_ERROR;
    }

    Q_UNUSED(lineIndex);

    VehiclePosition tmpVehiclePosition;
    tmpVehiclePosition.rotate = instructionIn.posR*M_PI/180.0;
    tmpVehiclePosition.x = instructionIn.posX;
    tmpVehiclePosition.y = instructionIn.posY;
    int returnFlag;
    programMcController->getControllerInterpreter()->setVehiclePosition(tmpVehiclePosition,D_SET_ODOMETER_COVE_RATIO,returnFlag);
    if(returnFlag != 1)
    {
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  12060, robotId
                , programEngine->getRunningLineFloat() );
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
    //可能需要等３ｓ
    sleep(3);

    programEngine->calculateNextLineToRun(programNumIn);

    return programEngine->isTheLastInstruction(programNumIn);
}

void SetOdomInstructionLogic::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int robotId, int parameter1, int parameter2, int parameter3, int parameter4, QString message)
{
    Q_UNUSED(parameter2);
    Q_UNUSED(parameter3);
    Q_UNUSED(parameter4);

    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;

    QString infomationStr;

    switch(messageCode)
    {

       case 12060:
       {

            infomationStr = QObject::tr("机器人:") +
                QString::number(robotId) +
                QObject::tr("第") +
                QString::number(parameter1+1) +
                QObject::tr("行运行出现异常:重置里程计失败!")+message;

            break;
        }

    default:
    {

    }
    }
    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);

}
